#include "pch.h"

#include "math/line3.h"



namespace Math
{

	///
	line3::line3():
m_point(Math::point4(0,0,0)),
	m_direction(Math::vector3(0,0,0))
{

}


///
line3::line3(const Math::point4 & p, const Math::vector3 & d):
m_point(p),
	m_direction(d)
{

}


///
line3::line3(const Math::point4 & p1, const Math::point4 & p2):
m_point(p1),
	m_direction(p2 - p1)
{

}


///
line3::line3(const Math::line3 & rhs):
m_point(rhs.m_point),
	m_direction(rhs.m_direction)
{

}



///
void line3::operator = (const Math::line3 & rhs)
{
	this->m_point = rhs.m_point;

	this->m_direction = rhs.m_direction;
}


///
bool line3::operator == (const Math::line3 & rhs) const 
{
	return ((this->m_point == rhs.m_point) &&
		(this->m_direction == rhs.m_direction));
}


///
bool line3::operator != (const Math::line3 & rhs) const 
{
	return ((this->m_point != rhs.m_point) ||
		(this->m_direction != rhs.m_direction));
}



///
scalar line3::length() const 
{
	return this->m_direction.length();
}


///
scalar line3::lengthsq() const
{
	return this->m_direction.lengthsq();
}

///
scalar line3::distance(const Math::point4 &p) const
{
	Math::vector3 difference(p - this->m_point);

	scalar s = float4::dot3(this->m_direction, this->m_direction);

	if(s > 0.0f)
	{
		scalar t = float4::dot3(this->m_direction, difference) / s;
		difference = difference - (this->m_direction * t);

		return difference.length();
	}
	else
	{
		Math::vector3 temp(p - this->m_point);

		return temp.length();
	}
}



///
void line3::set(const Math::point4 &p, const Math::vector3 &d)
{
	this->m_point = p;

	this->m_direction = d;
}


///
void line3::set(const Math::point4 &p1, const Math::point4 &p2)
{
	this->m_point = p1;

	this->m_direction = (p2 - p1);
}


///
const Math::point4 & line3::getstart() const 
{
	return this->m_point;
}


///
Math::point4 line3::getend() const 
{
	return (this->m_point + this->m_direction);
}



///
bool line3::intersectline(const Math::point4 &p) const
{
	Math::float4 temp = p - this->getstart();

	if(float4::normalize(temp) == float4::normalize(this->m_direction))
		return true;
	else
		return false;
}


///
bool line3::intersectline(const Math::line3 & l) const
{
	Math::float4   u = this->getend() - this->getstart();
	Math::float4   v = l.getend() - l.getstart();
	Math::float4   w = this->getstart() - l.getstart();
	scalar    a = float4::dot3(u,u);        // always >= 0
	scalar    b = float4::dot3(u,v);
	scalar    c = float4::dot3(v,v);        // always >= 0
	scalar    d = float4::dot3(u,w);
	scalar    e = float4::dot3(v,w);
	scalar    D = a*c - b*b;       // always >= 0
	scalar    sc, tc;

	// compute the line parameters of the two closest points
	if (D < R_TINY) 
	{         // the lines are almost parallel
		sc = 0.0;
		tc = (b>c ? d/b : e/c);   // use the largest denominator
	}
	else 
	{
		sc = (b*e - c*d) / D;
		tc = (a*e - b*d) / D;
	}

	// get the difference of the two closest points
	Math::vector3 dP = w + (u * sc) - (v * tc);  // = L1(sc) - L2(tc)

	if(dP != vector3::nullvec())
		return true;   // return the closest distance
	else
		return false;
}


///
scalar line3::getclosestpoint_t(const Math::point4 & p) const 
{
	scalar s = float4::dot3(this->m_direction, this->m_direction);

	if(s > 0.0f)
		return float4::dot3(this->m_direction, p - this->m_point) / s;
	else
		return 0.0f;
}


///
Math::point4 line3::getclosestpoint(const Math::point4 & p) const
{
	scalar s = float4::dot3(this->m_direction, this->m_direction);

	if(s > 0.0f)
		return this->getpointat(float4::dot3(this->m_direction, p - this->m_point) / s);
	else
		return this->m_point;
}


///
Math::point4 line3::getpointat(scalar t) const 
{
	return this->m_point + (this->m_direction * t);
}

}